#include <mathTools.h>
#include <math.h>

#include "ik.h"
#include "common.h"

#define POS_MAX 4
#define POS_01  0
#define POS_12  1
#define POS_23  2
#define POS_03  3

using namespace std;

IK_Obj::IK_Obj(Motor_Ctrl *ctrl, double l_abcd, double l_hip, double l_knee) : pmotor_ctrl(ctrl), L_abcd(l_abcd), L_hip(l_hip), L_knee(l_knee) 
{
    cout<<"IK_Obj"<<endl;
    for(uint8_t i=0; i<pmotor_ctrl->get_motor_num(); i++) {
        if(pmotor_ctrl->get_motor_item(i)->get_motor_dir() == DIR_CLOCKWISE) {
            this->ik_param.theta_dir[i] = DIR_COUNTER;
        } else {
            this->ik_param.theta_dir[i] = DIR_CLOCKWISE;
        }
    }
}

IK_Obj::~IK_Obj()
{
    cout<<"~IK_Obj"<<endl;
}

/* 已知角度求端点坐标 */
void IK_Obj::forward_kinematics()
{
    double angle[3];                //弧度
    double l1 = L_abcd;
    double l2 = (-L_hip);
    double l3 = (-L_knee);

    Eigen::Matrix<double, 4, 4> D_H[POS_MAX];   //D-H矩阵
    Eigen::Matrix<double, 3, 1> V;
    Eigen::Matrix<double, 4, 1> P;              
    Eigen::Matrix<double, 4, 1> tmp;

    angle[0] = this->get_ik_param()->angle[0]*PAI/180;
    angle[1] = this->get_ik_param()->angle[1]*PAI/180;
    angle[2] = this->get_ik_param()->angle[2]*PAI/180;

    D_H[POS_01].block(0, 0, 3 ,3) = rotx(angle[0]);
    D_H[POS_01].block(3, 0, 1, 3).setZero();
    D_H[POS_01].block(0, 3, 3, 1).setZero();
    D_H[POS_01](3, 3) = 1;

    D_H[POS_12].block(0, 0, 3 ,3) = roty(angle[1]);
    D_H[POS_12].block(3, 0, 1, 3).setZero();
    D_H[POS_12].block(0, 3, 3, 1).setZero();
    D_H[POS_12](3, 3) = 1;

    D_H[POS_23].block(0, 0, 3 ,3) = roty(angle[2]);
    D_H[POS_23].block(3, 0, 1, 3).setZero();
    D_H[POS_23](0, 3) = 0;
    D_H[POS_23](1, 3) = l1;
    D_H[POS_23](2, 3) = l2;
    D_H[POS_23](3, 3) = 1;
    
    D_H[POS_03] =  D_H[POS_01] * D_H[POS_12] * D_H[POS_23];

    // std::cout << "D_H01:" << std::endl << D_H[POS_01] << std::endl;
    // std::cout << "D_H12:" << std::endl << D_H[POS_12] << std::endl;
    // std::cout << "D_H23:" << std::endl << D_H[POS_23] << std::endl;
    // std::cout << "D_H03:" << std::endl << D_H[POS_03] << std::endl;

#if 0
    V(0) = l3*sin(angle[1]+angle[2]) + l2*sin(angle[1]);
    V(1) = -l3*sin(angle[0])*cos(angle[1]+angle[2]) + l1*cos(angle[0]) - l2*cos(angle[1])*sin(angle[0]);
    V(2) = l3*cos(angle[0])*cos(angle[1]+angle[2]) + l1*sin(angle[0]) + l2*cos(angle[0])*cos(angle[1]);
    // std::cout << "V:" << std::endl << V << std::endl;
    this->get_ik_param()->pos[0] = V(0);
    this->get_ik_param()->pos[1] = V(1);
    this->get_ik_param()->pos[2] = V(2);
#else
    tmp << 0, 0, l3, 1;
    P = D_H[POS_03] * tmp;
    // std::cout << "P:" << std::endl << P << std::endl;
    this->get_ik_param()->pos[0] = P(0);
    this->get_ik_param()->pos[1] = P(1);
    this->get_ik_param()->pos[2] = P(2);
#endif

}

/* 已知端点坐标求角度 */
void IK_Obj::inverse_kinematics()
{
    double pos[3];
    double angle[3];
    double L;
    double l1 = L_abcd;
    double l2 = (-L_hip);
    double l3 = (-L_knee);
    
    pos[0] = this->get_ik_param()->pos[0];
    pos[1] = this->get_ik_param()->pos[1];
    pos[2] = this->get_ik_param()->pos[2]; 
    L = sqrt(pow(pos[1], 2) + pow(pos[2], 2) - pow(l1, 2));

    angle[0] = atan2(pos[2]*l1 + pos[1]*L, pos[1]*l1- pos[2]*L);
    angle[2] = -PAI + acos((pow(L_hip, 2) + pow(L_knee, 2) - (pow(pos[0], 2)+pow(pos[1], 2)+pow(pos[2], 2)-pow(L_abcd, 2)))/(2*L_hip*L_knee));
    
    double a1 = pos[1]*sin(angle[0]) - pos[2]*cos(angle[0]);
    double a2 = pos[0];
    double m1 = l3*sin(angle[2]);
    double m2 = l3*cos(angle[2]) + l2;
    angle[1] = atan2(a1*m1+a2*m2,a2*m1-a1*m2);

    // std::cout << "angle0:" << angle[0]*180/PAI << std::endl;
    // std::cout << "angle1:" << angle[1]*180/PAI << std::endl;
    // std::cout << "angle2:" << angle[2]*180/PAI << std::endl;

    this->get_ik_param()->angle[0] = angle[0]*180/PAI;
    this->get_ik_param()->angle[1] = angle[1]*180/PAI;
    this->get_ik_param()->angle[2] = angle[2]*180/PAI;
}


uint32_t IK_Obj::is_angle_valid()
{
    if((this->ik_param.angle[0] < (THETA0_MIN-THETA_ERROR)) || (this->ik_param.angle[0] > (THETA0_MAX+THETA_ERROR))) {
        return 0;
    }

    if((this->ik_param.angle[1] < (THETA1_MIN-THETA_ERROR)) || (this->ik_param.angle[1] > (THETA1_MAX+THETA_ERROR))) {
        return 0;
    }

    if((this->ik_param.angle[2] < (THETA2_MIN-THETA_ERROR)) || (this->ik_param.angle[2] > (THETA2_MAX+THETA_ERROR))) {
        return 0;
    }

    return 1;
}
